#!/usr/bin/env python  
import pygame  
import sys  
import rospy  
from sensor_msgs.msg import Image  
from cv_bridge import CvBridge  
from std_msgs.msg import String  
import numpy as np  
import cv2


def send_hit_command():  
    global hit_pub  
    #hit_pub.publish(f"HIT {id}")  
    #rospy.loginfo(f"Sent HIT command with ID: {id}")  
    print("attack confirmed!")
  
def send_track_command():  
    global track_pub  
    #track_pub.publish("TRACK")  
    rospy.loginfo("Sent TRACK command")  
image = np.zeros((720, 1280, 3), dtype=np.uint8)  
def image_callback(ros_image):  

    image_surf = bridge.imgmsg_to_cv2(ros_image, "bgr8")  
    #cv2.imshow("Image window", image_surf)
    #cv2.waitKey(3)
    global image
    image = image_surf


pygame.init()  
screen = pygame.display.set_mode((1280, 720))    
pygame.display.set_caption('ROS Image Display and Click Selection')  
clock = pygame.time.Clock()  
  
rospy.init_node('image_display_and_click', anonymous=True)  
  

image_topic = "/TopoCam/img"  
click_pub_topic = "/clicked_position"   
image_sub = rospy.Subscriber(image_topic, Image, callback=image_callback, queue_size=1)  
click_pub = rospy.Publisher(click_pub_topic, String, queue_size=10)  
  

bridge = CvBridge()  
  
# follow circle
show_circle = False
GREEN = (0, 255, 0)  
BLACK = (0, 0, 0)  
font = pygame.font.SysFont('arial', 36)  

circle_pos = (350, 250)  
circle_radius = 50  
  

show_circle = False  

SLIDER_BAR_WIDTH = 300  
SLIDER_BAR_HEIGHT = 30  
SLIDER_BUTTON_WIDTH = 30  
 
SLIDER_MAX_VALUE = 100  


slider_image = pygame.image.load('confirm.png').convert_alpha()  
cancel_image = pygame.image.load('cancel.png').convert_alpha() 
SLIDER_WIDTH = 40  
SLIDER_HEIGHT = 40 
scaled_image = pygame.transform.scale(slider_image, (SLIDER_WIDTH, SLIDER_HEIGHT))  

show_slider = False  
slider_value = 0  
slider_confirmed = False  

dragging = False
SCREEN_HEIGHT  = 720
SCREEN_WIDTH = 1280
SLIDER_START_Y = SCREEN_HEIGHT/2  
SLIDER_START_X = SCREEN_WIDTH/2
slider_pos = [SCREEN_WIDTH // 2 - SLIDER_WIDTH // 2, SCREEN_HEIGHT // 2 - SLIDER_HEIGHT // 2]  

def draw_circle(surface, color, pos, radius, width=2):  
    pygame.draw.circle(surface, color, pos, radius, width)  
  
def draw_text_in_circle(surface, text, font, color, pos, radius):  
    text_obj = font.render(text, True, color)  
    text_rect = text_obj.get_rect(center=pos)  
    surface.blit(text_obj, text_rect)
  
slider_pos = [0,0]
confirm_pos = [0,0]
while not rospy.is_shutdown():  
    for event in pygame.event.get():  
        if event.type == pygame.QUIT:  
            pygame.quit()  
            sys.exit()  
        elif event.type == pygame.MOUSEBUTTONDOWN:  

            if event.button == 1:  
                if show_circle == True:
                    show_circle = not show_circle
                pos = pygame.mouse.get_pos()  
                click_msg = "Clicked at x={}, y={}".format(pos[0], pos[1])  
                rospy.loginfo(click_msg)  
                click_pub.publish(click_msg)  
                
                if (show_slider==False): 
                    if (pos[0]<20):
                        slider_pos[0] = 20
                    elif (pos[0]>1060):
                        slider_pos[0] = 1060
                    else:
                        slider_pos[0] = pos[0]
                    if (pos[1]>50):
                        slider_pos[1] = pos[1]-50
                    else:
                        slider_pos[1] = 0
                    show_slider = True
                    confirm_pos[0] = pos[0]-20

                    confirm_pos[1] = slider_pos[1]
                dragging = True 
                
               
            if event.button == 3: 
                if show_slider == True:
                    show_slider = False
                else:
                    show_circle = not show_circle

          
        elif event.type == pygame.MOUSEMOTION:  
            pos = pygame.mouse.get_pos() 
            """
            print(confirm_pos[0])
            print(slider_pos[1])
            print("pos")
            print(pos)
            print(show_slider)
            print(dragging)
            print(pos[0])
            print(confirm_pos[0])
            print(pos[0] > (confirm_pos[0]-20))
            print( pos[0] < (confirm_pos[0]-20) )
            print(pos[1])
            print(slider_pos[1])
            print(pos[1] > (slider_pos[1]-20+50))
            print(pos[1] < (slider_pos[1]+20+50))
            """
            if show_slider and dragging and pos[0] > (confirm_pos[0]) and pos[0] < (confirm_pos[0]+40) and pos[1] > (slider_pos[1]) and pos[1] < (slider_pos[1]+40) :  
                if (pos[0]<slider_pos[0]):
                    confirm_pos[0] = slider_pos[0]-20
                elif (pos[0]>slider_pos[0]+200):
                    confirm_pos[0] = slider_pos[0]+180
                else:
                    confirm_pos[0] = pos[0]-20
                confirm_pos[1] = slider_pos[1]
                # confirm
                if pos[0] > (slider_pos[0]+200):
                    slider_confirmed = True
                    show_slider = False
                    dragging = False
            else:
                confirm_pos = [slider_pos[0]-20,slider_pos[1]]

                
    if image is not None:  
        surface = pygame.surfarray.make_surface(np.rot90(cv2.cvtColor(image, cv2.COLOR_BGR2RGB) , 1))  
        screen.blit(surface, (0, 0)) 
    if show_slider:  
    #rgb246,128,128
        SLIDER_COLOR = (246,128,128)

        slider_pos_x = SLIDER_START_X + int((slider_value / SLIDER_MAX_VALUE) * (SLIDER_BAR_WIDTH - SLIDER_BUTTON_WIDTH))  

        pygame.draw.rect(screen, (246,128,128,208), (slider_pos[0], slider_pos[1], 200, 40))  
        # 绘制左边的圆弧  
        pygame.draw.circle(screen, (246,128,128), (slider_pos[0],slider_pos[1]+20), 20)  
        # 绘制右边的圆弧  
        pygame.draw.circle(screen, (246,128,128), (slider_pos[0]+200,slider_pos[1]+20), 20)  

   # 绘制文本  
   # 字体和大小  
        FONT_NAME = 'arial'  
        FONT_SIZE = 15  
        font = pygame.font.SysFont(FONT_NAME, FONT_SIZE)  
        text = font.render('SLIDE TO CONFIRM', True, (250,255,255,255))  
        text_rect = text.get_rect(center=(slider_pos[0]+100, slider_pos[1]+20))  
        screen.blit(text, text_rect)  

        if dragging:
            screen.blit(scaled_image, confirm_pos) 
        else:
        
            confirm_pos_init = [slider_pos[0]-20,slider_pos[1]]
            screen.blit(scaled_image, confirm_pos_init) 
    if slider_confirmed:  
        send_hit_command()    
        show_slider = False  
        slider_confirmed = False 
    if show_circle:  
        draw_circle(screen, GREEN, circle_pos, circle_radius)  
        draw_text_in_circle(screen, "Follow", font, BLACK, circle_pos, circle_radius)  

    pygame.display.flip()  
    clock.tick(30)
